
#include "rviz_custom_plugins/utils/pcl_util.h"
#include <pcl/common/centroid.h>
#include <pcl/common/common.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Matrix3x3.h>
#include <QJsonDocument>
#include <QJsonObject>


namespace rviz_custom_plugins
{

namespace utils
{


geometry_msgs::msg::Quaternion eigenMatrix3fToQuaternion(const Eigen::Matrix3f& rot)
{
    tf2::Matrix3x3 tfRot(
        rot(0,0), rot(0,1), rot(0,2),
        rot(1,0), rot(1,1), rot(1,2),
        rot(2,0), rot(2,1), rot(2,2)
    );
    tf2::Quaternion q;
    tfRot.getRotation(q);

    geometry_msgs::msg::Quaternion quaternion;
    quaternion.x = q.x();
    quaternion.y = q.y();
    quaternion.z = q.z();
    quaternion.w = q.w();
    return quaternion;
}


bool fitPlane(pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclPcd, Eigen::Vector4f& coefficients)
{
    if (pclPcd->size() < 3)
        return false;

    // 计算质心
    Eigen::Vector4f centroid;
    pcl::compute3DCentroid(*pclPcd, centroid);

    // 计算协方差矩阵
    Eigen::Matrix3f covariance;
    pcl::computeCovarianceMatrixNormalized(*pclPcd, centroid, covariance);

    // 求特征值和特征向量
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigenSolver(covariance, Eigen::ComputeEigenvectors);
    Eigen::Vector3f normal = eigenSolver.eigenvectors().col(0);  // 最小特征值对应的特征向量为法向量

    // 得到平面系数 ax + by + cz + d = 0
    float d = -normal.dot(centroid.head<3>());
    coefficients.head<3>() = normal;
    coefficients[3] = d;

    return true;

}


pcl::PointCloud<pcl::PointXYZINormal>::Ptr projectPointsToPlane(
    const pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclPcd,
    const Eigen::Vector4f& coefficients)
{
    float a = coefficients[0];
    float b = coefficients[1];
    float c = coefficients[2];
    float d = coefficients[3];
    float denom = a * a + b * b + c * c;

    pcl::PointCloud<pcl::PointXYZINormal>::Ptr projectedPcd(new pcl::PointCloud<pcl::PointXYZINormal>);
    projectedPcd->reserve(pclPcd->size());

    for (const auto& pt : pclPcd->points) {
        float dist = (a * pt.x + b * pt.y + c * pt.z + d) / denom;
        pcl::PointXYZINormal proj;
        proj.x = pt.x - dist * a;
        proj.y = pt.y - dist * b;
        proj.z = pt.z - dist * c;
        projectedPcd->push_back(proj);
    }

    return projectedPcd;

}



QString pclPointToStr(const pcl::PointXYZINormal& point)
{
    QJsonObject jsonPoint;

    // 添加坐标信息
    jsonPoint["x"] = point.x;
    jsonPoint["y"] = point.y;
    jsonPoint["z"] = point.z;

    // 添加强度信息
    jsonPoint["intensity"] = point.intensity;

    // 添加法线信息
    jsonPoint["normal_x"] = (point.normal_x);
    jsonPoint["normal_y"] = (point.normal_y);
    jsonPoint["normal_z"] = (point.normal_z);

    // 添加曲率信息
    jsonPoint["curvature"] = point.curvature;

    QJsonDocument jsonDoc(jsonPoint);
    QString jsonStr = jsonDoc.toJson(QJsonDocument::Compact);
    return jsonStr;
}


pcl::PointXYZINormal strToPclPoint(const QString& jsonStr)
{
    QJsonDocument doc = QJsonDocument::fromJson(jsonStr.toUtf8());
    QJsonObject jsonPoint = doc.object();

    pcl::PointXYZINormal point;

    // 设置坐标
    point.x = jsonPoint["x"].toDouble();
    point.y = jsonPoint["y"].toDouble();
    point.z = jsonPoint["z"].toDouble();

    // 设置强度
    point.intensity = jsonPoint["intensity"].toDouble();

    // 设置法线
    point.normal_x = jsonPoint["normal_x"].toDouble();
    point.normal_y = jsonPoint["normal_y"].toDouble();
    point.normal_z = jsonPoint["nromal_z"].toDouble();

    // 设置曲率
    point.curvature = jsonPoint["curvature"].toDouble();

    return point;

}


}

}
